
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_position_control_param.c
  * @author     baiyang
  * @date       2021-10-3
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mc_position_control.h"

#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
//位置控制参数
static param_t var_info[] = {
    PARAM_DEFINE_FLOAT(PSC_POSZ_P, 1.0f),
    
    PARAM_DEFINE_FLOAT(PSC_VELZ_P, 5.0f),
    PARAM_DEFINE_FLOAT(PSC_VELZ_I, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_VELZ_D, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_VELZ_IMAX, 1000.0f),
    PARAM_DEFINE_FLOAT(PSC_VELZ_FF, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_VELZ_FLTE, 5.0f),
    PARAM_DEFINE_FLOAT(PSC_VELZ_FLTD, 5.0f),
    
    PARAM_DEFINE_FLOAT(PSC_ACCZ_P, 0.5f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_I, 1.0f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_D, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_IMAX, 800.0f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_FF, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_FLTT, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_FLTE, 20.0f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_FLTD, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_SMAX, 0.0f),
    
    PARAM_DEFINE_FLOAT(PSC_POSXY_P, 1.0f),
    
    PARAM_DEFINE_FLOAT(PSC_VELXY_P, 2.0f),
    PARAM_DEFINE_FLOAT(PSC_VELXY_I, 1.0f),
    PARAM_DEFINE_FLOAT(PSC_VELXY_D, 0.5f),
    PARAM_DEFINE_FLOAT(PSC_VELXY_IMAX, 1000.0f),
    PARAM_DEFINE_FLOAT(PSC_VELXY_FF, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_VELXY_FLTE, 5.0f),
    PARAM_DEFINE_FLOAT(PSC_VELXY_FLTD, 5.0f),

    PARAM_DEFINE_FLOAT(PSC_ANGLE_MAX, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_JERK_XY, 5.0f),
    PARAM_DEFINE_FLOAT(PSC_JERK_Z, 5.0f),
};
PARAM_GROUP_DEFINE(POS_CTRL, var_info);
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]   pos_ctrl  
  * @param[out]  
  * @retval      
  * @note        
  */
void posctrl_assign_param(Position_ctrl* pos_ctrl)
{
    param_link_variable(PARAM_ID(POS_CTRL, PSC_POSZ_P), &pos_ctrl->_p_pos_z._kp);

    param_link_variable(PARAM_ID(POS_CTRL, PSC_VELZ_P), &pos_ctrl->_pid_vel_z._kp);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_VELZ_I), &pos_ctrl->_pid_vel_z._ki);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_VELZ_D), &pos_ctrl->_pid_vel_z._kd);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_VELZ_IMAX), &pos_ctrl->_pid_vel_z._kimax);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_VELZ_FF), &pos_ctrl->_pid_vel_z._kff);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_VELZ_FLTE), &pos_ctrl->_pid_vel_z._filt_E_hz);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_VELZ_FLTD), &pos_ctrl->_pid_vel_z._filt_D_hz);

    param_link_variable(PARAM_ID(POS_CTRL, PSC_ACCZ_P), &pos_ctrl->_pid_accel_z._kp);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_ACCZ_I), &pos_ctrl->_pid_accel_z._ki);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_ACCZ_D), &pos_ctrl->_pid_accel_z._kd);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_ACCZ_IMAX), &pos_ctrl->_pid_accel_z._kimax);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_ACCZ_FF), &pos_ctrl->_pid_accel_z._kff);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_ACCZ_FLTT), &pos_ctrl->_pid_accel_z._filt_t_hz);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_VELZ_FLTE), &pos_ctrl->_pid_accel_z._filt_e_hz);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_ACCZ_FLTD), &pos_ctrl->_pid_accel_z._filt_d_hz);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_ACCZ_SMAX), &pos_ctrl->_pid_accel_z._slew_rate_max);

    param_link_variable(PARAM_ID(POS_CTRL, PSC_POSXY_P), &pos_ctrl->_p_pos_xy._kp);

    param_link_variable(PARAM_ID(POS_CTRL, PSC_VELXY_P), &pos_ctrl->_pid_vel_xy._kp);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_VELXY_I), &pos_ctrl->_pid_vel_xy._ki);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_VELXY_D), &pos_ctrl->_pid_vel_xy._kd);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_VELXY_IMAX), &pos_ctrl->_pid_vel_xy._kimax);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_VELXY_FF), &pos_ctrl->_pid_vel_xy._kff);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_VELXY_FLTE), &pos_ctrl->_pid_vel_xy._filt_E_hz);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_VELXY_FLTD), &pos_ctrl->_pid_vel_xy._filt_D_hz);

    param_link_variable(PARAM_ID(POS_CTRL, PSC_ANGLE_MAX), &pos_ctrl->_lean_angle_max);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_JERK_XY), &pos_ctrl->_shaping_jerk_xy);
    param_link_variable(PARAM_ID(POS_CTRL, PSC_JERK_Z), &pos_ctrl->_shaping_jerk_z);
}
/*------------------------------------test------------------------------------*/


